# coding : utf-8
"""
以下几个工作
1.自由空间
1.1 红绿灯
1.2 停止线
1.3 静态障碍物
1.4 道路黄实线

2.动态障碍物
2.1 物体跟踪预测

"""
import math

import carla

from agents.tools.misc import is_within_distance_ahead
from agents.navigation.lattice_planner import PhysicsRoadOption

# def get_distance(wp0:carla.Waypoint, wp1:carla.Waypoint):
#     dist = 0
#     loc0 = wp0.transform.location
#     loc1 = wp1.transform.location
#     dist = math.sqrt((loc0)
#     return dist

def get_cuvature(wp0:carla.Waypoint, wp1:carla.Waypoint):
    cuv = 0
    return cuv

class FreeSpaceDealer:
    """
    把静态障碍物、交通灯、交通标志等变换为道路可撒点宽度
    """
    def __init__(self, vehicle, local_trace, sample_resolution):
        self._vehicle = vehicle
        self._local_trace = local_trace
        self._sample_resolution = sample_resolution
        pass

    def traffic_sign(self):
        """
        处理交通标志，包括红绿灯
        """
        # 判断当前路径上是否有红绿灯
        light_state = self._vehicle.get_traffic_light_state() # 不知道能管多远，是不是真
        # 如果有红绿灯，寻找停车线位置
        if light_state == 'Red' or light_state == 'Unknown':
            pass
        # 判断当前路径上是否有禁止变道的黄实线或者白实线
        pass

    def static_obstacles(self):
        """
        输出最终的障碍物信息，把道路标线也当成障碍物，红绿灯也当成障碍物
        """
        # 过滤后方的障碍物
        # 
        pass

    def road_space(self) -> list(carla.Location, PhysicsRoadOption):
        """
            :
            caution: 因为sample_resolution足够小(<0.5m)，才能用这种方法。
            因为这个假定车辆位置就在waypoint上
        """
        s_trace = None
        for wp, road_option in self._local_trace:
            current_physics_road_option = PhysicsRoadOption()
            current_physics_road_option.slength += self._sample_resolution
            current_physics_road_option.option = road_option
            s_trace.append((wp, current_physics_road_option))
        return s_trace

    def is_crosswalk(self, waypoint:carla.Waypoint):
        """
        检查路点是否在人行横道上
        """
        return False

    def is_stopline(self, waypoint:carla.Waypoint):
        """
        检查路点是否是停止线
        """
        return False